#ifndef __LPA__
#define __LPA__


#include <iostream>
#include <vector>
#include <set>
#include <algorithm>
#include <limits>
#include <cmath>

#include "meta_search.hpp"
#include "D_star.hpp"

namespace graph_search{
    class lpa:public search{
        private:
            double cost(const N& s1,const N& s2){
                return 1;
                }
            bool isValid(int x,int y,int n){
                return x>=0 && y>=0 && x<n && y<n;
            }
            int heuristic(int x1,int y1,int x2,int y2){
                return abs(x1-x2) + abs(y1-y2);
            }
            void update_vertex(const N& s,const N& start, const N& goal,
                            const std::vector<std::vector<int>>& grid,
                            std::vector<std::vector<N>>& nodes,
                            std::set<N>& openSet){
                if(!isValid(s.x,s.y,grid.size()) || s==start){
                    return;
                }

                double costMin = std::numeric_limits<double>::max();
                for(int i=0;i<4;i++){
                    const int x = s.x + dx[i],y = s.y+ dy[i];
                    if(isValid(x,y,grid.size()) && grid[x][y] == 0){
                        costMin = min(costMin,nodes[x][y].g + cost(nodes[x][y],s));
                    }
                }

                nodes[s.x][s.y].rhs = costMin;
                openSet.erase(s);
                if(nodes[s.x][s.y].g != nodes[s.x][s.y].rhs){
                    openSet.insert(s);
                }
            }
        public:
            int find(const std::vector<std::vector<int>>& matrix,Position startPoint,Position endPoint){
                int n = grid.size();
                N start(startPoint.x,startPoint.y);
                N goal(endPoint.x,endPoint.y);

                std::vector<std::vector<N>> nodes(n,std::vector<N>(n,N(0,0)));

                for(int x=0;x<n;x++){
                    for(int y=0;y<n;y++){
                        nodes[y][x] = Node(y,x);
                    }
                }

                std::set<N> openSet;

                nodes[start.y][start.x].rhs = 0;
                openSet.insert(start);

                while(!openSet.empty() && (openSet.begin()->rhs != openSet.begin()->g || goal < *openSet.begin())){
                    Node selected = *openSet.begin();
                    openSet.erase(openSet.begin());

                    if(selected.g != selected.rhs){
                        for(int i=0;i<4;++i){
                            const int x = selected.x + dx[i];
                            const int y = selected.y + dy[i];
                            if(isValid(x,y,grid.size()) && grid[y][x] == 0){
                                update_vertex(nodes[y][x],start,goal,grid,nodes,openSet);
                            } 
                        }
                    } else {
                        nodes[selected.y][selectd.x].g = numeric_limits<double>::max();
                        update_vertex(selected,start,goal,grid,nodes,openSet);
                        for(int i=0;i<4;++i){
                            const int x = selected.x + dx[i];
                            const int y = selected.y + dy[i];
                            if(isValid(x,y,grid.size()) && grid[y][x] == 0){
                                update_vertex(nodes[y][x],start,goal,grid,nodes,openSet);
                            } 
                        }
                    }
                }
            }
    };
}

#endif